clear all
tic
global n A B shipID WP3DStandard FasterFasterYES WaterDepth
global undercoverPenalty2 SeaTemp earliestArrival additionalOffTime
global draught trim pointIdentifiers ETA p q maplegend DepthLimit
global avgSpeed latestArrival undercoverPenalty WaveHeightLimit f1 f2
global plotThatShit TimeResolution npoints Resolution stormTime
global WaveDir CurrentSpeed CurrentDir WindSpeed WindDir WaveHeight

n = 4;
A = [24.48, 118.09, 0];
B = [17.96, 257.81, 360];
shipID = 71;
draught = 8;
trim = 0;

%PENALTIES
ETAPenalty = 5;
WavePenalty = 2;
LandPenalty = 0.1;
undercoverPenalty = ETAPenalty;
undercoverPenalty2 = 0;

FasterFasterYES = 1;

%PASSING OPTIONS FOR THE MAIN DIRECT OPTIMIZATION
options.maxits = 5000;
options.maxevals = 20000;
options.maxdeep = 10000;
options.ep = 10^-12;

%SECONDARY VARIABLES
WaveHeightLimit = 9;
DepthLimit = -20;
maxSpeed = 25;
SeaTemp = 15;

%COMPUTATIONAL VARIABLES
offTime = 0;
additionalOffTime = 5;
Nlat = 56;
Wlong = 114;
Resolution = 1;
maplegend = [Resolution, Nlat, Wlong];
TimeResolution = 6;
npoints = 20;

latBounds = [16, 55];
longDivide = (B(2)-A(2))/n;
latestArrival = B(3) + offTime;
earliestArrival = B(3) - offTime;
Sizing = size(WaveHeight);
WP3DStandard = zeros(Sizing(1), Sizing(2), Sizing(3));
f1=[16,55,Sizing(1)];
f2=[116,260,Sizing(2)];

%OPTIONS FOR PREPROCESSED DIRECT OPTIMIZATION
optionsSimple.maxits = 2500;
optionsSimple.maxevals = 6000;

%PREPROCESSING TO DETERMINE UNDISTURBED AVERAGE SAILING SPEED
plotThatShit = 0;

%DEFINING BOUNDS FOR SEARCHED VARIABLES FOR NO WEATHER OR SPEED ROUTE
for i = 1:n
    boundsSimple((i-1)*2+1,1) = latBounds(1);
    boundsSimple((i-1)*2+1,2) = latBounds(2);
    boundsSimple((i-1)*2+2,1) = A(2) + (i-1)*longDivide;
    boundsSimple((i-1)*2+2,2) = A(2) + (i)*longDivide;
end

%PASSING MINIMIZATION PROBLEM, CONSTRAINTS, BOUNDS AND OPTIONS TO DIRECT
ProblemSimple.f = 'MinimumDistance';
ProblemSimple.numconstraints = 1;
ProblemSimple.constraint(1).func = 'LandConstraintSimple';
ProblemSimple.constraint(1).penalty = LandPenalty;
[fminSimple, xminSimple, histSimple] = Direct(ProblemSimple, boundsSimple, optionsSimple);

avgSpeed = fminSimple/B(3);

%PREPROCESSING THE SHIP SPECIFIC CURVES TO IMPROVE OPTIMIZATION SPEED
if FasterFasterYES == 1
    powertune = zeros(1, 101);
    powertune(1) = 0;
    for i = 1:100
        powertune(i+1) = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    end
    x = linspace(0.25, 25, 100);
    x = [0, x];
    
    p = polyfit(x(37:end), powertune(37:end), 3);
    
    %GETTING THE POWER TO FUEL CONSUMPTION CURVE FOR FASTER CALCULATIONS
    fueltune = zeros(1, 101);
    fueltune(1) = 0;
    for i = 1:100
        fueltune(i+1) = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    end
    
    q = polyfit(powertune, fueltune, 1);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%MAIN OPTIMIZATION CALL

%Ensures the average speed for an undisturbed route is centered in the
%speedinterval.
avgSpeed = 20.3972;
minSpeed = 2*avgSpeed - maxSpeed;

plotThatShit = 1;

%DEFINING BOUNDS FOR SEARCHED VARIABLES FOR MAIN PROBLEM
bounds = zeros(n*3+1, 2);
bounds(n*3+1, 1) = minSpeed;
bounds(n*3+1, 2) = maxSpeed;
for i = 1:n
    bounds((i-1)*3+1, 1) = latBounds(1);
    bounds((i-1)*3+1, 2) = latBounds(2);
    bounds((i-1)*3+2, 1) = A(2) + (i-1)*longDivide;
    bounds((i-1)*3+2, 2) = A(2) + (i)*longDivide;
    bounds((i-1)*3+3, 1) = minSpeed;
    bounds((i-1)*3+3, 2) = maxSpeed;
end

%PASSING MINIMIZATION PROBLEM, CONSTRAINTS, BOUNDS AND OPTIONS TO DIRECT
Problem.f = 'TotalFuelConsumption';
Problem.numconstraints = 3;
Problem.constraint(1).func = 'ETAConstraint';
Problem.constraint(1).penalty = ETAPenalty;
Problem.constraint(2).func = 'LandConstraint';
Problem.constraint(2).penalty = LandPenalty;
Problem.constraint(3).func = 'MaxWaveHeightConstraint';
Problem.constraint(3).penalty = WavePenalty;
[fmin, xmin, hist] = Direct(Problem, bounds, options);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%VISUALISATION OF THE SOLUTION
toc

%SOLUTION PLOTTING
pointIdentifiers = zeros(n+2, 4);
pointIdentifiers(1, 1) = A(1);
pointIdentifiers(1, 2) = A(2);
pointIdentifiers(1, 3) = 0;
pointIdentifiers(1, 4) = 0;
ETA = 0;
for i = 2:n+1
    pointIdentifiers(i, 1) = xmin((i-2)*3 + 1);
    pointIdentifiers(i, 2) = xmin((i-2)*3 + 2);
    pointIdentifiers(i, 3) = xmin((i-2)*3 + 3);
    ETA = ETA + 60*distance(pointIdentifiers(i-1, 1),...
        pointIdentifiers(i-1, 2), pointIdentifiers(i, 1),...
        pointIdentifiers(i, 2))/pointIdentifiers(i, 3);
    pointIdentifiers(i, 4) = ETA;
end
pointIdentifiers(n+2, 1) = B(1);
pointIdentifiers(n+2, 2) = B(2);
pointIdentifiers(n+2, 3) = xmin(n*3 + 1);
pointIdentifiers(n+2, 4) = pointIdentifiers(n+1, 4)...
    + 60*distance(pointIdentifiers(n+1, 1), pointIdentifiers(n+1, 2),...
    pointIdentifiers(n+2, 1), pointIdentifiers(n+2, 2))/...
    pointIdentifiers(n+2, 3);
disp(pointIdentifiers)

figure(1)
[latlim, longlim] = limitm(WaterDepth, maplegend);
worldmap(latlim, longlim)
meshm(WaterDepth, maplegend, size(WaterDepth), WaterDepth)
demcmap(WaterDepth)
hold on
plotm(pointIdentifiers(:, 1), pointIdentifiers(:, 2), 'b', 'LineWidth', 2)
plotm(pointIdentifiers(:, 1), pointIdentifiers(:, 2), 'xr', 'MarkerSize',...
    15, 'LineWidth', 2)

figure(2)
plot(hist(:, 2), hist(:, 3), 'xr')
xlabel('Number of funciton evaluations.');
ylabel('Minimum functional value at evaluation');
title('Iteration Statistics');
